{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "9fbbd778",
   "metadata": {
    "cell_id": "d7fa06e2-b83c-4434-8d29-d135297e1d76",
    "colab_type": "text",
    "deepnote_cell_height": 142.796875,
    "deepnote_cell_type": "markdown",
    "id": "TKvYiJgnYExi"
   },
   "source": [
    "# Drake practice - \"Direct\" Joint Angle Teleop\n",
    "First, we provide a reference to the first example notebook of chapter 1. Then, in the bottom cell, you will find a similar function with slightly different functionality, and code missing that you are required to fill in. Take a look below for further instructions!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f5d27eb6",
   "metadata": {
    "cell_id": "82eab780f5bf4914b990fef54d54cddc",
    "deepnote_cell_height": 251.984375,
    "deepnote_cell_type": "markdown"
   },
   "source": [
    "## Review: 2D End-effector Teleop (from Chapter 1)\n",
    "\n",
    "In the chapter 1 [example](https://manipulation.csail.mit.edu/intro.html#teleop2d), we assembled a diagram with the manipulation station, a meshcat visualizer, some systems that provide a minimal teleop interface, along with some systems to convert the teleop output from end-effector commands into joint commands.\n",
    "\n",
    "In this problem, your goal is to build an even simpler diagram, which instead directly controls the joint angles of the robot.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "62c402b6",
   "metadata": {
    "cell_id": "b887e38d72bf441684f82ebb5ee86dac",
    "deepnote_cell_height": 108.390625,
    "deepnote_cell_type": "markdown"
   },
   "source": [
    "## Setup\n",
    "Imports from drake and starting the Meshcat viewer."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "32b406b9",
   "metadata": {
    "cell_id": "00001-43d4d95e-5231-4e73-a3df-0f924b87488b",
    "colab": {},
    "colab_type": "code",
    "deepnote_cell_height": 220,
    "deepnote_cell_type": "code",
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 823,
    "execution_start": 1662685910770,
    "id": "A4QOaw_zYLfI",
    "lines_to_end_of_cell_marker": 2,
    "source_hash": "293fc6da"
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.geometry import StartMeshcat\n",
    "from pydrake.systems.analysis import Simulator\n",
    "\n",
    "from manipulation import running_as_notebook\n",
    "from manipulation.exercises.grader import Grader\n",
    "from manipulation.exercises.robot.test_direct_joint_control import (\n",
    "    TestDirectJointControl,\n",
    ")\n",
    "from manipulation.station import MakeHardwareStation, load_scenario"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "29f1d64c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()\n",
    "meshcat.Set2dRenderMode(xmin=-0.25, xmax=1.5, ymin=-0.1, ymax=1.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4a3ceb66",
   "metadata": {
    "cell_id": "00002-f5df043d-5e95-4ce5-a6a4-c4abec79c1fa",
    "deepnote_cell_height": 452.578125,
    "deepnote_cell_type": "code",
    "deepnote_output_heights": [
     null,
     20.1875
    ],
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 6783,
    "execution_start": 1662685912388,
    "output_cleared": false,
    "source_hash": "4ce270d5"
   },
   "outputs": [],
   "source": [
    "scenario_data = \"\"\"\n",
    "directives:\n",
    "- add_model:\n",
    "    name: iiwa\n",
    "    file: package://manipulation/planar_iiwa14_no_collision.urdf\n",
    "    default_joint_positions:\n",
    "        iiwa_joint_2: [0.1]\n",
    "        iiwa_joint_4: [-1.2]\n",
    "        iiwa_joint_6: [1.6]\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: iiwa::iiwa_link_0\n",
    "model_drivers:\n",
    "    iiwa: !IiwaDriver {}\n",
    "\"\"\"\n",
    "scenario = load_scenario(data=scenario_data)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ab057643",
   "metadata": {
    "cell_id": "5459ae55e14f48c7a28340ec1123044a",
    "deepnote_cell_height": 380.578125,
    "deepnote_cell_type": "markdown"
   },
   "source": [
    "## Directly control the robot joints\n",
    "You'll now get a chance to replace the teleop functionality in the 2D example above with a direct \"joint angle setter\".\n",
    "\n",
    "More specifically, we want you to complete the `teleop_2d_direct` function below, such that a user can directly pass in a desired angle of each joint and have the robot move to the commanded joint angles. You will have to make use of some drake functionality that's not used in the `teleop_2d` function (see [`FixValue`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_input_port.html#ab285168d3a19d8ed367e11053aec79c3) and [`CreateDefaultContext`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_system.html#ad047317ab91889c6743d5e47a64c7f08)) and you can leave out all the components that were used in `teleop_2d` which are no longer needed.\n",
    "\n",
    "### The goals of this exercise are twofold:\n",
    "- Understand the way core subsystems in Drake are wired together well enough to know which lines in the  `teleop_2d` function must be removed.\n",
    "- Use the information in the examples from the textbook, other exercises, and Drake documentation, to correctly implement the same high-level joint-space control behavior, but using a different Drake function.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0ecc4a9e",
   "metadata": {
    "cell_id": "b710309fcb124497b3804f3f16e54180",
    "deepnote_cell_height": 1467,
    "deepnote_cell_type": "code",
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 1,
    "execution_start": 1662686083452,
    "output_cleared": true,
    "source_hash": "14ad6cfc"
   },
   "outputs": [],
   "source": [
    "def teleop_2d_direct(interactive=False, q_cmd=np.zeros(3)):\n",
    "    \"\"\"\n",
    "    Joint position control of the Kuka iiwa robot, without using teleop sliders or differential IK.\n",
    "\n",
    "    Args:\n",
    "        interactive (bool): If True, function will query the user to manually set the desired joint positions\n",
    "            while running the simulator. Otherwise, function will use \"q_cmd\" as the target joint position.\n",
    "        q_cmd (np.ndarray): Shape (3,). Desired positions of the three movable joints on the 2D robot.\n",
    "            \"q_cmd\" cannot be None if \"interactive\" is set to False.\n",
    "    \"\"\"\n",
    "    assert (\n",
    "        not interactive and q_cmd is not None\n",
    "    ) or interactive, 'Variable \"q_cmd\" must not be None if the function is run in non-interactive mode'\n",
    "\n",
    "    station = MakeHardwareStation(scenario, meshcat=meshcat)\n",
    "\n",
    "    #######################################################################\n",
    "    # Your code here. Setup the simulation and context. Make sure that the\n",
    "    # context variable is set to the context owned by the simulator.\n",
    "\n",
    "    simulator = None\n",
    "    context = None\n",
    "\n",
    "    #######################################################################\n",
    "\n",
    "    if simulator is None:\n",
    "        print(\"You must set the simulator variable above\")\n",
    "        return station, context\n",
    "\n",
    "    simulator.set_target_realtime_rate(1.0 if interactive else 0)\n",
    "    meshcat.AddButton(\"Stop Simulation\")\n",
    "    while meshcat.GetButtonClicks(\"Stop Simulation\") < 1:\n",
    "        simulator.AdvanceTo(simulator.get_context().get_time() + 2.0)\n",
    "\n",
    "        #######################################################################\n",
    "        # Your code here\n",
    "        # (read the current measured joint angles into the variable `q_current`)\n",
    "        # (hint: what output ports does the `station` instance have available?)\n",
    "\n",
    "        q_current = np.zeros(3)\n",
    "        print(f\"Current joint angles: {q_current}\")\n",
    "\n",
    "        #######################################################################\n",
    "\n",
    "        if interactive and running_as_notebook:\n",
    "            q_cmd = q_current.copy()\n",
    "            q_cmd[0] = float(\n",
    "                input(\n",
    "                    f\"Please enter value for first movable joint (current value: {q_current[0]:.3f})\"\n",
    "                )\n",
    "            )\n",
    "            q_cmd[1] = float(\n",
    "                input(\n",
    "                    f\"Please enter value for second movable joint (current value: {q_current[1]:.3f})\"\n",
    "                )\n",
    "            )\n",
    "            q_cmd[2] = input(\n",
    "                f\"Please enter value for third movable joint (current value: {q_current[2]:.3f})\"\n",
    "            )\n",
    "\n",
    "        #######################################################################\n",
    "        # Your code here\n",
    "        # (command the desired joint positions, and read the joint angle command into variable `q_current_cmd`)\n",
    "\n",
    "        q_current_cmd = np.zeros(3)\n",
    "        print(f\"Current commanded joint angles: {q_current_cmd}\\n\")\n",
    "\n",
    "        #######################################################################\n",
    "\n",
    "        if not interactive or not running_as_notebook:\n",
    "            break\n",
    "\n",
    "    meshcat.DeleteButton(\"Stop Simulation\")\n",
    "\n",
    "    return station, context"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cc2da8e1",
   "metadata": {
    "cell_id": "bdb08ce3ec55484fa926f7c737fe0df8",
    "deepnote_cell_height": 74.796875,
    "deepnote_cell_type": "markdown"
   },
   "source": [
    "Run the cell below to use the function you wrote in an interactive mode, but note that the autograder will test the functionality of your code in non-interactive mode."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "287ab6c6",
   "metadata": {
    "cell_id": "f2787cc89f2d40138577a4cb042f5f13",
    "deepnote_cell_height": 139.1875,
    "deepnote_cell_type": "code",
    "deepnote_output_heights": [
     20.1875
    ],
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 539,
    "execution_start": 1662686089330,
    "source_hash": "92d39d17"
   },
   "outputs": [],
   "source": [
    "teleop_2d_direct(interactive=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7ee9b518",
   "metadata": {
    "cell_id": "3ec7b8ff2863406ba2b7e7354fac85c5",
    "deepnote_cell_height": 62,
    "deepnote_cell_type": "markdown"
   },
   "source": [
    "## How will this notebook be Graded?\n",
    "\n",
    "If you are enrolled in the class, this notebook will be graded using [Gradescope](www.gradescope.com). You should have gotten the enrollement code on our announcement in Piazza.\n",
    "\n",
    "For submission of this assignment, you must do as follows:.\n",
    "- Download and submit the notebook `direct_joint_control.ipynb` to Gradescope's notebook submission section, along with your notebook for the other problems.\n",
    "\n",
    "We will evaluate the local functions in the notebook to see if the function behaves as we have expected. For this exercise, the rubric is as follows:\n",
    "- [5pts] `teleop_2d_direct` must be implemented correctly (i.e., we will check if you are properly sending position commands to the robot)\n",
    "\n",
    "Below is our autograder where you can check your score!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c9c2d00a",
   "metadata": {
    "cell_id": "166f1a5849d04d28879d91a537afecaf",
    "deepnote_cell_height": 347.46875,
    "deepnote_cell_type": "code",
    "deepnote_output_heights": [
     365.703125
    ],
    "deepnote_to_be_reexecuted": false,
    "execution_millis": 5,
    "execution_start": 1662686169319,
    "output_cleared": false,
    "source_hash": "addab6f"
   },
   "outputs": [],
   "source": [
    "Grader.grade_output([TestDirectJointControl], [locals()], \"results.json\")\n",
    "Grader.print_test_results(\"results.json\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.10 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}